/* 
 * File: main_lab2_display.c
 * Author: Testuya Idota and Brian Fehrman
 * Description: Sends commands to the iRobot
 * 				from the Seeduino via USART
 * 				communication that request different
 * 				status updates from the robot. The
 * 				different robot statuses are sent
 * 				to transmitted via USART0 in a 
 * 				human readable format.
 * Created on (date): 2-5-2013
 */
#include <avr/io.h>
#include <util/delay.h>
#include <inttypes.h>
#include <avr/interrupt.h>
//#include "usart.h"
//#include "iRobotSensors.h"
//#include "iRobotControl.h"

void init();
void drive_forward();
void turn_left();
void turn_right();

int main (void)
{    
	//Initialize USART0 and USART1
	
	//ioinit();
	init();
	//Reconfigure stdout and stdin to call our functions
	//stdout = &my_std_out;
	//stdin = &my_std_in;
	

	while(1){
		
		drive_forward();

		if( ( PINH & 0b01001000 ) )
		{
			turn_left();
			while( PINH & 0b01001000 );
		}
		if( ( PINH & 0b00100000 ) )
		{
			turn_right();
			while(  PINH & 0b00100000 );
		}

	}

	return -1;
}

void init()
{
	DDRB = 0xFF;
	PORTB = 0xFF;
	DDRH = 0b00010000;
	PORTH |= 0b00010000;
	DDRG = 0xFF;
	DDRE = 0xFF;
	
	sei();
	
	TCCR0A = 0xA3;
	TCCR0B = 0x05;
	
	TCNT0 = 0x00;
}

void drive_forward()
{
	PORTB = 0b00000000;
	PORTE = 0b00000000;
	OCR0A = 255;
	OCR0B = 255;
}

void turn_right()
{
	PORTB |= 0b01000000;
	PORTE = 0b00000000;
	OCR0A = 255;
	OCR0B = 255;
}

void turn_left()
{
	PORTB = 0b00000000;
	PORTE |= 0b00100000;
	OCR0A = 255;
	OCR0B = 255;
}

